import painting.Painter;

public class MazeRobot extends Thread {
	remoteApi vrep;
	int clientID;
	int leftMotor;
	int rightMotor;
	int frontRight;
	int sideRight;
	int frontLeft;
	int sideLeft;
	private Painter painter;
	private int dr20;
	public MazeRobot(Painter painter) {
		this.painter = painter;
	}

	public void run() {
		 vrep = new remoteApi();
		 clientID = vrep.simxStart("127.0.0.1", 21000, true, true, 5000, 5);
		if (clientID != -1) {
			System.out.println("Connected to remote API server");
			IntW handle = new IntW(-1);
			int a = vrep.simxGetObjectHandle(clientID, "dr20_leftWheelJoint_",
					handle, vrep.simx_opmode_oneshot_wait);
			 leftMotor = handle.w;
			if (a == vrep.simx_error_noerror) {
				System.out.println(leftMotor);
			}
			handle.w = -1;
			int b = vrep.simxGetObjectHandle(clientID, "dr20_rightWheelJoint_",
					handle, vrep.simx_opmode_oneshot_wait);
			 rightMotor = handle.w;
			handle.w = -1;
			int c = vrep.simxGetObjectHandle(clientID, "dr20_infraredSensor1_",
					handle, vrep.simx_opmode_oneshot_wait);
			 frontRight = handle.w;
			handle.w = -1;
			int d = vrep.simxGetObjectHandle(clientID, "dr20_infraredSensor2_",
					handle, vrep.simx_opmode_oneshot_wait);
			 sideRight = handle.w;
			handle.w = -1;
			int e = vrep.simxGetObjectHandle(clientID, "dr20_infraredSensor6_",
					handle, vrep.simx_opmode_oneshot_wait);
			 frontLeft = handle.w;
			handle.w = -1;
			int f = vrep.simxGetObjectHandle(clientID, "dr20_infraredSensor5_",
					handle, vrep.simx_opmode_oneshot_wait);
			 sideLeft = handle.w;
			 
			 handle.w = -1;
				int g = vrep.simxGetObjectHandle(clientID, "dr20",
						handle, vrep.simx_opmode_oneshot_wait);
				 dr20 = handle.w;
				 
				 handle.w = -1;
					int h = vrep.simxGetObjectHandle(clientID, "dr20_ultrasonicSensor_",
							handle, vrep.simx_opmode_oneshot_wait);
					 int front = handle.w;
			 
			 
			// simxGetVisionSensorDepthBuffer  Need to look a bit more on this one.



	

			System.out.println(c + " " + b + " " + c + " " + d + " " + e + " "
					+ f);
			boolean sleept = false;
			long LastSleept = 0;
			int count  = 0;
			while (vrep.simxGetConnectionId(clientID) != -1) {
				BoolW bw = new BoolW(false);
				FloatWA detectionsPoint = new FloatWA(3);
				vrep.simxReadProximitySensor(clientID, frontRight, bw, detectionsPoint, null, null, vrep.simx_opmode_streaming);
				BoolW bw2 = new BoolW(false);
				FloatWA detectionsPoint2 = new FloatWA(3);
				vrep.simxReadProximitySensor(clientID, sideRight, bw2, detectionsPoint2, null, null, vrep.simx_opmode_streaming);
				BoolW bw3 = new BoolW(false);
				FloatWA detectionsPoint3 = new FloatWA(3);
				vrep.simxReadProximitySensor(clientID, frontLeft, bw3, detectionsPoint3, null, null, vrep.simx_opmode_streaming);
				BoolW bw4 = new BoolW(false);
				FloatWA detectionsPoint4 = new FloatWA(3);
				vrep.simxReadProximitySensor(clientID, sideLeft, bw4, detectionsPoint4, null, null, vrep.simx_opmode_streaming);
				
				BoolW bw5 = new BoolW(false);
				FloatWA detectionsPoint5 = new FloatWA(3);
				vrep.simxReadProximitySensor(clientID, front, bw5, detectionsPoint5, null, null, vrep.simx_opmode_streaming);
				
				//temp to make it turn
//				long sleep = 100;
//				if (bw.w == true) {
//					System.out.println(detectionsPoint.w[0] + " " + detectionsPoint.w[1] + " " + detectionsPoint.w[2]);
//					sleep = 3000;
//				}else if(bw2.w == true){
//					System.out.println(detectionsPoint2.w[0] + " " + detectionsPoint2.w[1] + " " + detectionsPoint2.w[2]);
//					sleep = 3000;
//				}else if(bw3.w == true){
//					System.out.println(detectionsPoint3.w[0] + " " + detectionsPoint3.w[1] + " " + detectionsPoint3.w[2]);
//					sleep = 3000;
//				}else if(bw4.w == true){
//					System.out.println(detectionsPoint4.w[0] + " " + detectionsPoint4.w[1] + " " + detectionsPoint4.w[2]);
//					sleep = 3000;
//				}
				FloatWA sideRightPosition = new FloatWA(3);
				vrep.simxGetObjectPosition(clientID,dr20, -1, sideRightPosition, vrep.simx_opmode_streaming);
				//System.out.println("position: " + sideRightPosition.w[0] + " " + sideRightPosition.w[1] + " " + sideRightPosition.w[2]);
				int leftCount = 0;
				int rightCount = 0;
				int xPixels = Math.round(-sideRightPosition.w[0]*100);
				int yPixels = Math.round(sideRightPosition.w[1]*100);
				painter.addBlackSquare(500 + xPixels, 500 + yPixels);
				if (bw.w == true) {
					int xdetect = Math.round(-sideRightPosition.w[0]*100 + -detectionsPoint.w[0]*1000);
					int ydetect = Math.round(sideRightPosition.w[1]*100 + detectionsPoint.w[1]*1000);
					//painter.addRedSquare(500 + xdetect, 500 + ydetect);
					//sleep = 3000;
					System.out.println(detectionsPoint.w[0] + " " + detectionsPoint.w[1] + " " + detectionsPoint.w[2]);
					if( detectionsPoint.w[2] < (float)0.4){
						//System.out.println(detectionsPoint.w[0] + " " + detectionsPoint.w[1] + " " + detectionsPoint.w[2]);
						rightCount++;
					}
				}
				if(bw2.w == true){
					System.out.println(detectionsPoint2.w[0] + " " + detectionsPoint2.w[1] + " " + detectionsPoint2.w[2]);
					int xdetect = Math.round(-sideRightPosition.w[0]*100 + -detectionsPoint2.w[0]*1000);
					int ydetect = Math.round(sideRightPosition.w[1]*100 + detectionsPoint2.w[1]*1000);
					//painter.addRedSquare(500 + xdetect, 500 + ydetect);
					//sleep = 3000;
					if( detectionsPoint2.w[2] < (float)0.3){  // Only count if we are close! No need to if we still are far away from the object! Need to do the same for the others, works a bit differently.
						//rightCount++;
						//System.out.println(detectionsPoint2.w[0] + " " + detectionsPoint2.w[1] + " " + detectionsPoint2.w[2]);
					}
				}
				 if(bw3.w == true){
						int xdetect = Math.round(-sideRightPosition.w[0]*100 + -detectionsPoint3.w[0]*1000);
						int ydetect = Math.round(sideRightPosition.w[1]*100 + detectionsPoint3.w[1]*1000);
						//painter.addRedSquare(500 + xdetect, 500 + ydetect);
					 System.out.println(detectionsPoint3.w[0] + " " + detectionsPoint3.w[1] + " " + detectionsPoint3.w[2]);
					//sleep = 3000;
					if( detectionsPoint3.w[2] < (float)0.4){
						
						leftCount++;
						//
					}
				}
				if(bw4.w == true){
					System.out.println(detectionsPoint4.w[0] + " " + detectionsPoint4.w[1] + " " + detectionsPoint4.w[2]);
					//sleep = 3000;
					int xdetect = Math.round(-sideRightPosition.w[0]*100 + -detectionsPoint4.w[0]*1000);
					int ydetect = Math.round(sideRightPosition.w[1]*100 + detectionsPoint4.w[1]*1000);
					//painter.addRedSquare(500 + xdetect, 500 + ydetect);
					if( detectionsPoint4.w[2] < (float)0.3){
						//leftCount++;
						//
					}
				}
				
				
				
				
				if(bw4.w != true){
					if( System.currentTimeMillis() - LastSleept > 4000 || sleept == false ){
						sleept = true;
						try {
							sleep(400);
							turnLeft();
							sleep(1000);
							forward();
							LastSleept = System.currentTimeMillis();
							//sleep(1500);
						} catch (InterruptedException e1) {
							// TODO Auto-generated catch block
							e1.printStackTrace();
					}
					
					}

				}
				if(detectionsPoint3.w[2] < (float)0.3 && detectionsPoint.w[2] < (float)0.3 && bw5.w == true && detectionsPoint5.w[2] < (float)0.3){
					turnRight();
//					try {
//						sleep(900);
//					} catch (InterruptedException e1) {
//						e1.printStackTrace();
//					}
				}else if(detectionsPoint.w[2] < (float)0.25 && bw.w == true ){
					
					turnLeft();

				}else if(detectionsPoint3.w[2] < (float)0.25 && bw3.w == true){
					turnRight();
				}else if (bw5.w == true && detectionsPoint5.w[2] < (float)0.3){
					turnRight();
				}else{
					forward();
				}
					
				
				
				
				
				
				
				
				
				
				
				//Currently it will turn directly if and objects is in the way! As it can detect things far away it will turn i circles! Need to check distance to determine when to change direction. That is if we have a narrow corridor we want it to go in to it! Se the actual sensor positions for a visual view why it would not.
				
				
				
				
				



			}

		}
	}

	private void forward() {
		//System.out.println("forward");
		vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) 3,vrep.simx_opmode_oneshot);
		vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) 3, vrep.simx_opmode_oneshot);
		
	}

	private void turnAround() {
		//System.out.println("back");
		turnRight(); // Or turn left?
		try {
			sleep(1700);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		
	}

	private void turnRight() {
		//System.out.println("right");
		vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) 3,vrep.simx_opmode_oneshot);
		vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) -3, vrep.simx_opmode_oneshot);
		
	}

	private void turnLeft() {
		//System.out.println("left");
		vrep.simxSetJointTargetVelocity(clientID, leftMotor, (float) -3,vrep.simx_opmode_oneshot);
		vrep.simxSetJointTargetVelocity(clientID, rightMotor,(float) 3, vrep.simx_opmode_oneshot);
		
	}
}

